/*
 * trdp_mini_vos.c
 *
 *  Created on: 2016骞�12鏈�20鏃�
 *      Author: think
 */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stddef.h>
#include <stdint.h>
#include <unistd.h>
#include <sys/stat.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <netinet/in.h>
#include <pthread.h>
#include <fcntl.h>
#include <memory.h>
#include <net/if.h>
#include <ifaddrs.h>
#include <errno.h>
#include <sched.h>

#include "../inc/trdp_mini_vos.h"
#include "../inc/trdp_mini_proto.h"
#include "../api/trdp_mini.h"
#include "../config/trdp_mini_config.h"


#ifndef PROGMEM
#define PROGMEM
#define pgm_read_dword(a)  (*(a))
#endif


INT8U				gIsRedPortActive = 0;
INT32U				gLocolTopoCnt = 0;
static INT8U		memoryLocker = 0;
static pthread_mutex_t	trdpThreadMutex;
static int			sock = -1, sockMD = -1;
static int			interfaceCnt = 0;
struct in_addr 		out_if[TRDP_MINI_INTERFACE_CNT];

static void *thread_TCP_receiver(void *arg);
static void *thread_trdp_mini_process(void *arg);
static void *thread_UDP_receiver(void *arg);

/***********************************************************************************************************************
 *  LOCALS
 */
/** Table of CRC-32s of all single-byte values according to IEEE802.3
 *  The FCS-32 generator polynomial:
 *  x**0 + x**1 + x**2 + x**4 + x**5 + x**7 + x**8 + x**10 + x**11 + x**12 + x**16
 *        + x**22 + x**23 + x**26 + x**32.
 */

static const INT32U crc_table[256] PROGMEM =
{
    0x00000000, 0x77073096, 0xee0e612c, 0x990951ba,
    0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3,
    0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988,
    0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91,
    0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de,
    0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
    0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec,
    0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5,
    0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172,
    0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,
    0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940,
    0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
    0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116,
    0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f,
    0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924,
    0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,
    0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a,
    0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
    0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818,
    0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01,
    0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e,
    0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457,
    0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c,
    0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
    0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2,
    0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb,
    0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0,
    0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9,
    0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086,
    0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
    0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4,
    0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad,
    0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a,
    0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683,
    0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8,
    0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
    0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe,
    0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7,
    0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc,
    0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5,
    0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252,
    0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
    0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60,
    0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79,
    0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236,
    0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f,
    0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04,
    0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
    0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a,
    0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713,
    0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38,
    0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21,
    0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e,
    0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
    0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c,
    0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45,
    0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2,
    0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db,
    0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0,
    0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
    0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6,
    0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf,
    0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94,
    0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d
};

static void *thread_UDP_receiver(void *arg);
static void *thread_trdp_mini_process(void *arg);
/**********************************************************************************************************************/
/** Compute crc32 according to IEEE802.3.
 *  Note: Returned CRC is inverted
 *
 *  @param[in]          crc         Initial value.
 *  @param[in,out]      pData       Pointer to data.
 *  @param[in]          dataLen     length in bytes of data.
 *  @retval             crc32 according to IEEE802.3
 */

INT32U vos_crc32 (INT32U crc, const INT8U *pData, INT32U dataLen)
{
    INT32U i;
    for (i = 0; i < dataLen; i++)
    {
        crc = (crc >> 8) ^ pgm_read_dword(&crc_table[(crc ^ pData[i]) & 0xff]);
    }
    return ~crc;
}

/*
*********************************************************************************************************
*                                        TAKE THE MEMORY DATA SEMAPHORE
*
* Description: This function take the memory data Semaphore for data's atom operation.
*
* Arguments  : None
*
* Returns    : OK :	get the Semaphore successfully

*              ERROR : get the Semaphore failed
*********************************************************************************************************
*/
INT8U	vos_MemSemaphoreTake(void)
{
	/*
	 * user should use the specified OS resource to rewrite this function
	 * like xSemaphoreTake ...
	 * if the program has no OS, you can use the default process like below
	 * */

	if(memoryLocker == 0){
		memoryLocker = 1;
		return OK;
	}

	return ERROR;
}

/*
*********************************************************************************************************
*                                        GIVE THE MEMORY DATA SEMAPHORE
*
* Description: This function give the memory data Semaphore for data's atom operation.
*
* Arguments  : None
*
* Returns    : None
*********************************************************************************************************
*/
void	vos_MemSemaphoreGive(void)
{
	/*
	 * user should use the specified OS resource to rewrite this function
	 * like xSemaphoreGive ...
	 * if the program has no OS, you can use the default process like below
	 * */
	if(memoryLocker == 1){
		memoryLocker = 0;
	}
}
/*
*********************************************************************************************************
*                                        TAKE THE PACKET CONTROL BLOCK SEMAPHORE
*
* Description: This function take the package control block Semaphore for data's atom operation.
*
* Arguments  : None
*
* Returns    : OK :	get the Semaphore successfully

*              ERROR : get the Semaphore failed
*********************************************************************************************************
*/
INT8U	vos_PcbSemaphoreTake(void)
{
	/*
	 * user should use the specified OS resource to rewrite this function
	 * like xSemaphoreTake ...
	 * if the program has no OS, you can use the default process like below
	 * */
	if(pthread_mutex_lock(&trdpThreadMutex) == 0 ){

		return OK;
	}

	return ERROR;
}

/*
*********************************************************************************************************
*                                        GIVE THE PACKET CONTROL BLOCK SEMAPHORE
*
* Description: This function give the package control block Semaphore for control block's atom operation.
*
* Arguments  : None
*
* Returns    : None
*********************************************************************************************************
*/
void	vos_PcbSemaphoreGive(void)
{
	/*
	 * user should use the specified OS resource to rewrite this function
	 * like xSemaphoreGive ...
	 * if the program has no OS, you can use the default process like below
	 * */
	pthread_mutex_unlock(&trdpThreadMutex);
}

/*
*********************************************************************************************************
*                                        GET MD Package Session ID
*
* Description: This function get a universal unique identifier for session ID
*
* Arguments  : None
*
* Returns    : None
*********************************************************************************************************
*/
INT8U vos_getSessionID(INT8U *pSessionID, INT32U qIdx)
{
	time_t timep;
	struct tm *p;

	if(pSessionID == NULL){
		return ERROR;
	}


	time(&timep);
	p=gmtime(&timep);
	pSessionID[0] = 1900+p->tm_year;			//year low byte
	pSessionID[1] = 1+p->tm_mon;				//month
	pSessionID[2] = p->tm_mday;					//day
	pSessionID[3] = p->tm_hour;					//hour
	pSessionID[4] = p->tm_min;					//minute
	pSessionID[5] = p->tm_sec;					//second

	pSessionID[6] = (INT8U)(gMsTimerCnt>>24);		//current tick which usually change every 1ms for the high high byte(currentTick >> 24)
	pSessionID[7] = (INT8U)(gMsTimerCnt>>16);		//current tick which usually change every 1ms for the high byte(currentTick >> 16)
	pSessionID[8] = (INT8U)(gMsTimerCnt>>8);		//current tick which usually change every 1ms for the high byte(currentTick >> 8)
	pSessionID[9] = (INT8U)gMsTimerCnt;
	pSessionID[10] = (qIdx >> 24) & 0x0F;
	pSessionID[11] = (qIdx >> 16) & 0x0F;
	pSessionID[12] = (qIdx >> 8) & 0x0F;
	pSessionID[13] = qIdx & 0x0F;

	return OK;
}

/*
*********************************************************************************************************
*                                        Send UDP Unicast message
*
* Description: This function should be overwrite by user to send a UDP Unicast package
*
* Arguments  : 	pSrcIP	:	source IP to send data
* 				pDestIP	:	Dest IP for the remote IP
* 				port	:	UDP communication port
* 				pData	:	UDP send data
* 				dataSize:	the length of data to send
*
* Returns    : 	OK		: 	send successfully
* 				ERROR	:	send failed
*********************************************************************************************************
*/
INT8U vos_sendUDPUnicast(INT8U *pSrcIP, INT8U *pDestIP, INT16U port, INT8U *pData, INT32U dataSize)
{
	INT32U srcIPAddress, destIPAddress;
	if(sock == -1){
		return ERROR;
	}
	if(sIsBigEndian != TRDP_MINI_USE_BIG_ENDIAN){
		srcIPAddress = trdp_mini_until_RvINT32U(*((INT32U *)pSrcIP));
		destIPAddress = trdp_mini_until_RvINT32U(*((INT32U *)pDestIP));
	}else{
		srcIPAddress = *((INT32U *)pSrcIP);
		destIPAddress = *((INT32U *)pDestIP);
	}

	return vos_sockSendUDP(sock, pData, &dataSize, srcIPAddress, destIPAddress, TRDP_MINI_PD_UDP_PORT);
}

/*
*********************************************************************************************************
*                                        Send UDP MultiCast message
*
* Description: This function should be overwrite by user to send a UDP Multicast package
*
* Arguments  : 	pSrcIP	:	source IP to send data
* 				pDestIP	:	Dest IP for the remote IP
* 				port	:	UDP communication port
* 				pData	:	UDP send data
* 				dataSize:	the length of data to send
*
* Returns    : 	OK		: 	send successfully
* 				ERROR	:	send failed
*********************************************************************************************************
*/
INT8U vos_sendUDPMulticast(INT8U *pSrcIP, INT8U *pDestIP, INT16U port, INT8U *pData, INT32U dataSize)
{
	INT32U srcIPAddress, destIPAddress;
	if(sock == -1){
		return ERROR;
	}

	if(sIsBigEndian != TRDP_MINI_USE_BIG_ENDIAN){
		srcIPAddress = trdp_mini_until_RvINT32U(*((INT32U *)pSrcIP));
		destIPAddress = trdp_mini_until_RvINT32U(*((INT32U *)pDestIP));
	}else{
		srcIPAddress = *((INT32U *)pSrcIP);
		destIPAddress = *((INT32U *)pDestIP);
	}

	return vos_sockSendUDP(sock, pData, &dataSize, srcIPAddress, destIPAddress, TRDP_MINI_PD_UDP_PORT);
}

/*
*********************************************************************************************************
*                                        Send UDP broadcast message
*
* Description: This function should be overwrite by user to send a UDP broadcast package
*
* Arguments  : 	pSrcIP	:	source IP to send data
* 				pDestIP	:	Dest IP for the remote IP
* 				port	:	UDP communication port
* 				pData	:	UDP send data
* 				dataSize:	the length of data to send
*
* Returns    : 	OK		: 	send successfully
* 				ERROR	:	send failed
*********************************************************************************************************
*/
INT8U vos_sendUDPBroadcast(INT8U *pSrcIP, INT8U *pDestIP, INT16U port, INT8U *pData, INT32U dataSize)
{
	return ERROR;
}

/*
*********************************************************************************************************
*                                        Send MD Package through TCP port
*
* Description: This function should be overwrite by user to send a UDP broadcast package
*
* Arguments  : 	pSrcIP	:	source IP to send data
* 				pDestIP	:	Dest IP for the remote IP
* 				port	:	UDP communication port
* 				pData	:	UDP send data
* 				dataSize:	the length of data to send
*
* Returns    : 	OK		: 	send successfully
* 				ERROR	:	send failed
*********************************************************************************************************
*/
INT8U vos_sendTCPMessage(INT8U *pSrcIP, INT8U *pDestIP, INT16U port, INT8U *pData, INT32U dataSize)
{
	INT32U srcIPAddress, destIPAddress;
	if(sockMD == -1){
		return ERROR;
	}

	if(sIsBigEndian != TRDP_MINI_USE_BIG_ENDIAN){
		srcIPAddress = trdp_mini_until_RvINT32U(*((INT32U *)pSrcIP));
		destIPAddress = trdp_mini_until_RvINT32U(*((INT32U *)pDestIP));
	}else{
		srcIPAddress = *((INT32U *)pSrcIP);
		destIPAddress = *((INT32U *)pDestIP);
	}

	return vos_sockSendUDP(sockMD, pData, &dataSize, srcIPAddress, destIPAddress, TRDP_MINI_MD_UDP_PORT);
}


/*
*********************************************************************************************************
*                                        ABORT TCP Connection
*
* Description: This function abort message package communication TCP connection
*
* Arguments  : None
*
* Returns    : None
*********************************************************************************************************
*/
INT8U	vos_abortTCPConnection(INT8U *pSrcIP, INT8U *pDestIP)
{
	return OK;
}

/*
 * user should complete the receive data function and call the trdp_mini_Receive_CallBack function to pass
 * the receive data to trdp_mini stack!!!!
 * */

/*-----------------*/

INT8U vos_init_recv_thread(void)
{
	pthread_t	threadID, trdpThreadID;

	pthread_attr_t      threadAttrib;
	struct sched_param  schedParam;  /* scheduling priority */
	int retCode;

	pthread_mutex_init(&trdpThreadMutex, NULL);

	retCode = pthread_attr_init(&threadAttrib);
	if (retCode != 0){
		return ERROR;
	}

	retCode = pthread_attr_setdetachstate(&threadAttrib, PTHREAD_CREATE_DETACHED);
	retCode = pthread_attr_setschedpolicy(&threadAttrib, (INT32S)254);
	schedParam.sched_priority = 254;
	retCode = pthread_attr_setschedparam(&threadAttrib, &schedParam);
	retCode = pthread_attr_setinheritsched(&threadAttrib, PTHREAD_EXPLICIT_SCHED);
	if (pthread_create(&threadID, &threadAttrib, thread_UDP_receiver, NULL) != 0) {
		return ERROR;
	}

	retCode = pthread_attr_setschedpolicy(&threadAttrib, (INT32S)255);
	schedParam.sched_priority = 255;
	retCode = pthread_attr_setschedparam(&threadAttrib, &schedParam);
	if (pthread_create(&trdpThreadID, &threadAttrib, thread_trdp_mini_process, NULL) != 0) {
		return ERROR;
	}

	retCode = pthread_attr_setschedpolicy(&threadAttrib, (INT32S)253);
	schedParam.sched_priority = 253;
	retCode = pthread_attr_setschedparam(&threadAttrib, &schedParam);
	if (pthread_create(&trdpThreadID, &threadAttrib, thread_TCP_receiver, NULL) != 0) {
		return ERROR;
	}

	retCode = pthread_attr_destroy(&threadAttrib);

	if(retCode != 0){
		return ERROR;
	}

	return OK;
}

INT8U vois_init_unicast_socket(INT8U qos, INT8U ttl)
{
	int sockOptValue = 0;
	struct sockaddr_in srcAddress;

	if ((sockMD = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1){
		return ERROR;
	}

	//set resuse addr port
	sockOptValue = 1;
	setsockopt(sockMD, SOL_SOCKET, SO_REUSEADDR, &sockOptValue, sizeof(sockOptValue));

	//set qos
	if ((qos > 0) && (qos < 8)){
		sockOptValue = (int) ((qos << 5) | 4);
		setsockopt(sockMD, IPPROTO_IP, IP_TOS, &sockOptValue, sizeof(sockOptValue));
	}

	//set ttl
	sockOptValue = ttl;
	setsockopt(sockMD, IPPROTO_IP, IP_TTL, &sockOptValue, sizeof(sockOptValue));


	sockOptValue = 1;
	//set broadcast enable
	setsockopt(sockMD, SOL_SOCKET, SO_BROADCAST, &sockOptValue, sizeof(sockOptValue));

	if (sockMD == -1){
		return ERROR;
	}

	memset((char *)&srcAddress, 0, sizeof(srcAddress));
	srcAddress.sin_family       = AF_INET;
	srcAddress.sin_addr.s_addr  = htonl(INADDR_ANY);
	srcAddress.sin_port         = htons(TRDP_MINI_MD_UDP_PORT);

	if (bind(sockMD, (struct sockaddr *)&srcAddress, sizeof(srcAddress)) == -1){
		return ERROR;
	}

	return OK;
}


INT8U vois_init_multicast_socket(INT8U ifCnt, INT8U mcCnt, const char * ifAddress[],  const char * mcAddress[], INT8U qos, INT8U ttl)
{
	int sockOptValue = 0, ifIdx=0, mcIdx = 0;
	struct sockaddr_in srcAddress;
	struct ip_mreq  mreq;
	//struct in_addr out_addr;

	if(ifCnt > TRDP_MINI_INTERFACE_CNT){
		return ERROR;
	}

	if ((sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1){
		return ERROR;
	}


	sockOptValue = 1;
	setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &sockOptValue, sizeof(sockOptValue));

	//set qos
	if ((qos > 0) && (qos < 8)){
		sockOptValue = (int) ((qos << 5) | 4);
		setsockopt(sock, IPPROTO_IP, IP_TOS, &sockOptValue, sizeof(sockOptValue));
	}

	//set ttl
	sockOptValue = ttl;
	setsockopt(sock, IPPROTO_IP, IP_MULTICAST_TTL, &sockOptValue, sizeof(sockOptValue));

	//set no multicast loop
	sockOptValue = 0;
	setsockopt(sock, IPPROTO_IP, IP_MULTICAST_LOOP, &sockOptValue, sizeof(sockOptValue));
	setsockopt(sock, IPPROTO_IP, IP_MULTICAST_ALL, (void*) &sockOptValue, sizeof(sockOptValue));

	if (sock == -1){
		return ERROR;
	}

	memset((char *)&srcAddress, 0, sizeof(srcAddress));
	srcAddress.sin_family       = AF_INET;
	srcAddress.sin_addr.s_addr  = htonl(INADDR_ANY);
	srcAddress.sin_port         = htons(TRDP_MINI_PD_UDP_PORT);

	if (bind(sock, (struct sockaddr *)&srcAddress, sizeof(srcAddress)) == -1){
		return ERROR;
	}


	for(ifIdx=0; ifIdx<ifCnt; ifIdx++){
		for(mcIdx=0; mcIdx<mcCnt; mcIdx++){
			mreq.imr_multiaddr.s_addr  = inet_addr(mcAddress[mcIdx]);
			mreq.imr_interface.s_addr  = inet_addr(ifAddress[ifIdx]);
			setsockopt(sock, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq));
		}
		out_if[ifIdx].s_addr = inet_addr(ifAddress[ifIdx]);
		setsockopt(sock, IPPROTO_IP, IP_MULTICAST_IF, (char *)&out_if[ifIdx],sizeof(out_if[ifIdx]));
	}

	interfaceCnt = ifCnt;

	return OK;

}

/**********************************************************************************************************************/
/** Send UDP data.
 *  Send data to the supplied address and port.
 *
 *  @param[in]      sock            socket descriptor
 *  @param[in]      pBuffer         pointer to data to send
 *  @param[in,out]  pSize           In: size of the data to send, Out: no of bytes sent
 *  @param[in]      ipAddress       destination IP
 *  @param[in]      port            destination port
 */

INT8U vos_sockSendUDP (
    INT32S       sock,
    const INT8U *pBuffer,
    INT32U      *pSize,
	INT32U		ifAddress,
	INT32U      ipAddress,
    INT16U      port)
{
    struct sockaddr_in destAddr;
    ssize_t sendSize    = -1;
    size_t  size        = 0;
    int ifIdx = 0;

    if (sock == -1 || pBuffer == NULL || pSize == NULL)
    {
        return ERROR;
    }

    size    = *pSize;
    *pSize  = 0;

    /*      We send UDP packets to the address  */
    memset(&destAddr, 0, sizeof(destAddr));
    destAddr.sin_family         = AF_INET;
    destAddr.sin_addr.s_addr    = htonl(ipAddress);
    destAddr.sin_port           = htons(port);

    for(ifIdx=0; ifIdx<interfaceCnt; ifIdx++){
    	if(out_if[ifIdx].s_addr ==  htonl(ifAddress)){
    		setsockopt(sock, IPPROTO_IP, IP_MULTICAST_IF, (char *)&out_if[ifIdx],sizeof(out_if[ifIdx]));
    		break;
    	}
    }

    do{
		   /*errno       = 0;*/
		   sendSize = sendto(sock,
							 (const char *)pBuffer,
							 size,
							 0,
							 (struct sockaddr *) &destAddr,
							 sizeof(destAddr));

		   if (sendSize >= 0)
		   {
			   *pSize += sendSize;
		   }

		   if (sendSize == -1 && errno == EWOULDBLOCK)
		   {
			   return ERROR;
		   }
	 }
	 while (sendSize == -1 && errno == EINTR);



    if (sendSize == -1)
    {
        return ERROR;
    }
    return OK;
}


/**********************************************************************************************************************/
/** Receive UDP data.
 *  The caller must provide a sufficient sized buffer. If the supplied buffer is smaller than the bytes received, *pSize
 *  will reflect the number of copied bytes and the call should be repeated until *pSize is 0 (zero).
 *  If the socket was created in blocking-mode (default), then this call will block and will only return if data has
 *  been received or the socket was closed or an error occured.
 *  If pointers are provided, source IP, source port and destination IP will be reported on return.
 *
 *  @param[in]      sock            socket descriptor
 *  @param[out]     pBuffer         pointer to applications data buffer
 *  @param[in,out]  pSize           pointer to the received data size
 *  @param[out]     pSrcIPAddr      pointer to source IP
 *  @param[out]     pSrcIPPort      pointer to source port
 */
INT8U vos_sockReceiveUDP (
    INT32S   sock,
    INT8U   *pBuffer,
    INT32U  *pSize,
    INT32U  *pSrcIPAddr,
    INT16U  *pSrcIPPort)
{
    union
    {
        struct cmsghdr  cm;
        char            raw[32];
    } control_un;
    struct sockaddr_in  srcAddr;
    socklen_t           sockLen = sizeof(srcAddr);
    ssize_t rcvSize = 0;
    struct msghdr       msg;
    struct iovec        iov;

    if (sock == -1 || pBuffer == NULL || pSize == NULL)
    {
        return ERROR;
    }

    /* clear our address buffers */
    memset(&msg, 0, sizeof(msg));
    memset(&control_un, 0, sizeof(control_un));

    /* fill the scatter/gather list with our data buffer */
    iov.iov_base    = pBuffer;
    iov.iov_len     = *pSize;

    /* fill the msg block for recvmsg */
    msg.msg_iov         = &iov;
    msg.msg_iovlen      = 1;
    msg.msg_name        = &srcAddr;
    msg.msg_namelen     = sockLen;
    msg.msg_control     = &control_un.cm;
    msg.msg_controllen  = sizeof(control_un);

    *pSize = 0;

    do
    {
        rcvSize = recvmsg(sock, &msg, 0);

        if (rcvSize != -1)
        {
            if (pSrcIPAddr != NULL)
            {
                *pSrcIPAddr = (uint32_t) ntohl(srcAddr.sin_addr.s_addr);
                /* vos_printLog(VOS_LOG_DBG, "udp message source IP: %s\n", vos_ipDotted(*pSrcIPAddr)); */
            }

            if (pSrcIPPort != NULL)
            {
                *pSrcIPPort = (INT16U) ntohs(srcAddr.sin_port);
            }
        }

        if (rcvSize == -1 && errno == EWOULDBLOCK)
        {
            return ERROR;
        }
    }
    while (rcvSize == -1 && errno == EINTR);


    if (rcvSize <= 0)
    {
        return ERROR;
    }
    else
    {
        *pSize = rcvSize;
        return OK;
    }
}

static void *thread_UDP_receiver(void *arg)
{
	static INT8U		udpData[1460];
	INT32U		size=sizeof(udpData), srcIPAddr=0;
	INT16U		srcIPPort = 0;

	memset(udpData, 0, sizeof(udpData));

	while(TRUE){

		if(sock != -1){
			if(vos_sockReceiveUDP(sock, udpData, &size, &srcIPAddr, &srcIPPort) == OK){
				srcIPAddr = trdp_mini_until_RvINT32U(srcIPAddr);
				trdp_mini_Receive_CallBack(NULL, udpData, size, (INT8U *)&srcIPAddr, srcIPPort);
				size=sizeof(udpData);
			}
		}
	}

	return 0;
}

static void *thread_TCP_receiver(void *arg)
{
	static INT8U		tcpData[1460];
	INT32U		size=sizeof(tcpData), srcIPAddr=0;
	INT16U		srcIPPort = 0;

	memset(tcpData, 0, sizeof(tcpData));

	while(TRUE){

		if(sock != -1){
			if(vos_sockReceiveUDP(sockMD, tcpData, &size, &srcIPAddr, &srcIPPort) == OK){
				srcIPAddr = trdp_mini_until_RvINT32U(srcIPAddr);
				trdp_mini_Receive_CallBack(NULL, tcpData, size, (INT8U *)&srcIPAddr, srcIPPort);
				size=sizeof(tcpData);
			}
		}
	}

	return 0;
}

static void *thread_trdp_mini_process(void *arg)
{
	while(TRUE){
		vos_threadDelay(950);
		gMsTimerCnt++;
	}

	return 0;
}

INT8U vos_threadDelay (INT32U delay)
{
    struct timespec wanted_delay;
    struct timespec remaining_delay;
    int ret;

    if (delay == 0)
    {
        /*    yield cpu to other processes   */
        if (sched_yield() != 0)
        {
            return ERROR;
        }
        return OK;
    }

    wanted_delay.tv_sec     = delay / 1000000;
    wanted_delay.tv_nsec    = (delay % 1000000) * 1000;
    do
    {
        ret = nanosleep(&wanted_delay, &remaining_delay);
        if (ret == -1 && errno == EINTR)
        {
            wanted_delay = remaining_delay;
        }
    }
    while (errno == EINTR);

    return OK;
}


/*-------------------------------------------end---------------------------------------------------------*/


